import sympy
import sympybotics
rbtdef = sympybotics.RobotDef('tx60l', # robot name
                          [('0', 0, 0, 'q'),
                           ('-pi/2', 0, 0, 'q-pi/2'),
                           ('0', 0.02, 0.4, 'q+pi/2'),
                           ('pi/2', 0.45, 0, 'q'),
                           ('-pi/2', 0, 0, 'q'),
                           ( 'pi/2', 0, 0, 'q')], # (alpha, a, d, theta)
                            dh_convention='modified' # either 'standard' or 'modified'
                              )
rbtdef.frictionmodel = {'Coulomb', 'viscous'} # options are None or a combination of 'Coulomb', 'viscous' and 'offset'
rbtdef.dynparms()
rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)
tau_str = sympybotics.robotcodegen.robot_code_to_func('C', rbt.invdyn_code, 'tau_out', 'tau', rbtdef)
print(tau_str)
f = open('tx60l_tau_out.cpp','w')
f.write(tau_str)
f.close()
rbt.calc_base_parms()
print(rbt.dyn.baseparms)
regressor_str = sympybotics.robotcodegen.robot_code_to_func('C', rbt.H_code, 'regressor_out', 'regressor', rbtdef)
print(regressor_str)
f = open('tx60l_regressor.cpp','w')
f.write(regressor_str)
f.close()
